//编码器输入，计算电机转速

#include <stdint.h>
#include <stdlib.h>
#include "Encoder.h"
#include "ti_msp_dl_config.h"  // 包含引脚定义

// 电机参数定义
#define MOTOR_GEAR_RATIO    20      // 减速比 1:20
#define MOTOR_PPR           13      // 每转脉冲数
#define ENCODER_MULTIPLIER  4       // 编码器倍频
#define PULSES_PER_REVOLUTION (MOTOR_PPR * ENCODER_MULTIPLIER * MOTOR_GEAR_RATIO)  // 每转输出轴的总脉冲数

//采集间隔时间
#define collect_time  100

// 简化的电机结构体，不使用特定硬件类型
typedef struct {
    uint32_t counter;      // 脉冲计数器
    int8_t dir;            // 方向 (1 or -1)
    uint32_t rpm;          // 每分钟转数
    uint32_t last_time;    // 上次计算时间
} Motor_TypeDef;

// 定义四个电机
static Motor_TypeDef motors[2] = {
    {0, 1, 0, 0},  // 电机1
    {0, 1, 0, 0},  // 电机2
};

void Encoder_Update(uint8_t motor_index, uint8_t direction)
{
    if(motor_index < 2) {
        motors[motor_index].dir = direction ? 1 : -1;
        motors[motor_index].counter++;  // 只增加计数，方向由dir决定
    }
}

void Encoder_Calculate_RPM(int16_t *motor1_rpm, int16_t *motor2_rpm)
{

    motors[0].rpm = (uint32_t)((motors[0].counter * 600) / (1040));//600为100ms乘10乘60，将结果换算成每分钟转速
    if (motor1_rpm)*motor1_rpm = (int16_t)motors[0].rpm * motors[0].dir*-1;
    motors[0].counter = 0; 

    motors[1].rpm = (uint32_t)((motors[1].counter * 600) / (1040));
    if (motor2_rpm)*motor2_rpm = (int16_t)motors[1].rpm * motors[1].dir;
    motors[1].counter = 0; 

}

/**
 * @brief 获取指定电机的RPM
 * @param motor_index 电机索引 (0-3)
 * @return 电机RPM值
 */
uint32_t Encoder_Get_RPM(uint8_t motor_index)
{
    if (motor_index < 2) {
        return motors[motor_index].rpm;
    }
    return 0;
}

int8_t Encoder_Get_Direction(uint8_t motor_index)
{
    if (motor_index < 2) {
        return motors[motor_index].dir;
    }
    return 0;
}

